/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2023 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "tim.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "can_if.h"
#include "timer.h"
#include "led.h"
#include "encoder.h"
#include "DC_Motor.h"
#include "button.h"
#include "event_os.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
typedef struct
{
	float speed;
	DC_Motor_t dc_motor;
	Quadrature_Encoder_t encode;
	Soft_WitchDog_t witchdog;
} Driver_Motor_t;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
LED_t led;
Driver_Motor_t dc_motor[4];
Motor_t *motor[4];
Soft_Timer_t timer_led, timer_button, timer_led_task, timer_id;
Massage_Pack_t can_rx_buffer[10];
Can_Handle_t can;
Button_t key;
int16_t id;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
#define USER_FLASH_BASE 0x803FC00
void flash_write(uint8_t *data, uint16_t size, uint16_t offset)
{
	uint16_t temp, longth = 0;
	if (offset + size >= 0x400)
		return;
	FLASH_EraseInitTypeDef flash_config;
	HAL_FLASH_Unlock();
	flash_config.TypeErase = FLASH_TYPEERASE_PAGES;
	flash_config.PageAddress = USER_FLASH_BASE;
	flash_config.NbPages = 1;
	uint32_t PageError = 0;
	HAL_FLASHEx_Erase(&flash_config, &PageError);
	temp = (size & 0xfff8) >> 3;
	while (temp--)
	{
		HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, USER_FLASH_BASE + offset + longth, *(uint64_t *)(data + longth));
		longth += 4;
	}
	temp = ((size & 0x0007) + 1) >> 1;
	while (temp--)
	{
		HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, USER_FLASH_BASE + offset + longth, *(uint16_t *)(data + longth));
		longth += 2;
	}
	HAL_FLASH_Lock(); // 锁住Flash
}

void Motor_Task(Event_Task_Handle_t const *const config)
{
	for (int i = 0; i < 4; i++)
	{
		Encoder_Update(&dc_motor[i].encode.encoder);
		Motor_Send_Data(motor[i], dc_motor[i].speed);
	}
}

void Can_Send_Task(Event_Task_Handle_t const *const config)
{
	static int8_t data[8];
	for (int i = 0; i < 4; i++)
	{
		data[2 * i] = constrain(Get_Motor_Position_Data(motor[i]) * 255 / (2 * PI), 255, 0);
		data[2 * i + 1] = constrain(Get_Motor_Speed_Data(motor[i]), 127, -128);
	}
	SEML_CAN_Send(&can, 0x310 + id, data, 8, CAN_RTR_DATA);
}

static uint32_t tx_conflict_tick;
void Can_Rx_Data(void *config, Massage_Pack_t const *const massage_pack)
{
	uint8_t *data, longth;
	longth = Get_Massage_Pack_Data(massage_pack, (void **)&data);
	// id冲突不接收新数据
	if (SEML_GetTick() - tx_conflict_tick < 2000)
		return;
	for (int i = 0; i < 4; i++)
	{
		dc_motor[i].speed = (int16_t)(data[2 * i] << 8) | data[2 * i + 1];
		SEML_Feed_WitchDog(&dc_motor[i]);
	}
}
void Display_Delay_Callback(void *config);
void Display_id_Callback(void *config);
void Set_id_Callback(void *config);
// id冲突常亮，速度给0
void Can_Tx_Conflict(void *config, Massage_Pack_t const *const massage_pack)
{
	for (int i = 0; i < 4; i++)
		dc_motor[i].speed = 0;
	tx_conflict_tick = SEML_GetTick();
	LED_Set_Mode(&led, LED_Twinkle_Mode);
	LED_Set_Period(&led, 1000);
	LED_Set_Duty(&led, 0.8f);
	SEML_Timer_Delete(&timer_id);
	SEML_Timer_Create(&timer_id, Timer_Single, 2000, Display_id_Callback, NULL);
}
static uint32_t click_tick;
/***
 * ID设置任务
 */
void Key_Long_Prass_Callback(Button_t *button)
{
	for (int i = 0; i < 4; i++)
	{
		dc_motor[i].speed += 200;
		if(dc_motor[i].speed>1000)
			dc_motor[i].speed = 1000;
		SEML_Feed_WitchDog(&dc_motor[i]);
	}
}
void Key_Press_Callback(Button_t *button)
{
	int16_t lest_id = *(int16_t *)(USER_FLASH_BASE + 0x100);
	// 第一次按下重设id
	if (SEML_GetTick() - click_tick > 1000)
	{
		id = -1;
	}
	// 注销接收回调 不接收新数据
	SEML_CAN_RxMassage_Logout(&can, 0x300 + lest_id);
	SEML_CAN_RxMassage_Logout(&can, 0x310 + lest_id);
	click_tick = SEML_GetTick();
	// 灯效关闭
	LED_Set_Mode(&led, LED_OFF_Mode);
	SEML_Timer_Delete(&timer_id);
	SEML_Timer_Create(&timer_id, Timer_Single, 500, Set_id_Callback, NULL);
}
void Key_Click_Callback(Button_t *button)
{
	id += Get_Button_PressCount(button);
}
void Set_id_Callback(void *config)
{
	if (id > 0 && id <= 8)
	{
		// 写入flash 提示更新完毕
		flash_write((uint8_t *)&id, 2, 0x100);
		LED_Set_Mode(&led, LED_Twinkle_Mode);
		LED_Locking_Phase(&led);
		LED_Set_Period(&led, 600);
		LED_Set_Duty(&led, 0.7f);
	}
	else
		id = *(uint16_t *)(USER_FLASH_BASE + 0x100);
	// 更新can接收id
	SEML_CAN_RxMassage_Register(&can, 0x300 + id, Can_Rx_Data, NULL);
	SEML_CAN_RxMassage_Register(&can, 0x310 + id, Can_Tx_Conflict, NULL);
	SEML_Timer_Create(&timer_id, Timer_Single, 1200 - 10, Display_id_Callback, NULL);
}

/***
 * LED显示任务
 */
void Display_id_Callback(void *config)
{
	// 设置灯效
	LED_Set_Mode(&led, LED_Twinkle_Mode);
	LED_Locking_Phase(&led);
	LED_Set_Period(&led, 333);
	LED_Set_Duty(&led, 0.3f);
	// 创建定时任务 闪烁完id数量后关闭
	SEML_Timer_Create(&timer_id, Timer_Single, 333 * id - 50, Display_Delay_Callback, NULL);
}
void Display_Delay_Callback(void *config)
{
	LED_Set_Mode(&led, LED_OFF_Mode);
	// 创建定时任务 关闭后闪烁id数量
	SEML_Timer_Create(&timer_id, Timer_Single, 1000, Display_id_Callback, NULL);
}
void rx_timeout_Callback(Driver_Motor_t *config)
{
	config->speed = 0;
}
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
 * @brief  The application entry point.
 * @retval int
 */
int main(void)
{
	/* USER CODE BEGIN 1 */
	id = *(int16_t *)(USER_FLASH_BASE + 0x100);
	/* USER CODE END 1 */

	/* MCU Configuration--------------------------------------------------------*/

	/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
	HAL_Init();

	/* USER CODE BEGIN Init */

	/* USER CODE END Init */

	/* Configure the system clock */
	SystemClock_Config();

	/* USER CODE BEGIN SysInit */

	/* USER CODE END SysInit */

	/* Initialize all configured peripherals */
	MX_GPIO_Init();
	MX_TIM1_Init();
	MX_TIM3_Init();
	MX_TIM5_Init();
	MX_TIM7_Init();
	MX_TIM2_Init();
	MX_TIM4_Init();
	MX_TIM8_Init();
	MX_CAN_Init();
	MX_TIM6_Init();
	/* USER CODE BEGIN 2 */
	// event os init
	Event_OS_Init();
	HAL_TIM_RegisterCallback(&htim6, HAL_TIM_PERIOD_ELAPSED_CB_ID, Event_OS_Callback);
	HAL_TIM_Base_Start_IT(&htim6);
	// software timer init
	HAL_TIM_RegisterCallback(&htim7, HAL_TIM_PERIOD_ELAPSED_CB_ID, SEML_Timer_Callback);
	HAL_TIM_Base_Start_IT(&htim7);
	SEML_Timer_Set_BaseFrec(200);
	// CAN init
	SEML_CAN_Init(&can, &hcan, can_rx_buffer, 10);
	SEML_CAN_RxMassage_Register(&can, 0x300 + id, Can_Rx_Data, NULL);
	SEML_CAN_RxMassage_Register(&can, 0x310 + id, Can_Tx_Conflict, NULL);
	// Button init
	SEML_Button_Init(&key, 0, KEY_GPIO_Port, KEY_Pin);
	SEML_Timer_Create(&timer_button, Timer_Cycle, 10, Button_Scan, NULL);
	Button_Register(&key, Press_Down, Key_Press_Callback);
	Button_Register(&key, Single_Clink, Key_Click_Callback);
	Button_Register(&key, Double_Clink, Key_Click_Callback);
	Button_Register(&key, Multiple_clicks, Key_Click_Callback);
	Button_Register(&key, Long_press_Release, Key_Long_Prass_Callback);
	// LED init
	SEML_Timer_Create(&timer_led, Timer_Cycle, 1, LED_Handle_Callback, NULL);
	LED_Init_GPIO(&led, LED_GPIO_Port, LED_Pin);
	LED_Set_Mode(&led, LED_OFF_Mode);
	LED_Set_Period(&led, 500);
	// motor driver init
	motor[0] = DC_Motor_Init(&dc_motor[0].dc_motor, &htim1, TIM_CHANNEL_4, &htim1, TIM_CHANNEL_3, 64000000);
	motor[1] = DC_Motor_Init(&dc_motor[1].dc_motor, &htim1, TIM_CHANNEL_2, &htim1, TIM_CHANNEL_1, 64000000);
	motor[2] = DC_Motor_Init(&dc_motor[2].dc_motor, &htim8, TIM_CHANNEL_4, &htim8, TIM_CHANNEL_3, 64000000);
	motor[3] = DC_Motor_Init(&dc_motor[3].dc_motor, &htim8, TIM_CHANNEL_2, &htim8, TIM_CHANNEL_1, 64000000);
	// motor encoder init
	Quadrature_Encoder_Init(&dc_motor[0].encode, motor[0], &htim3, 100, 1);
	Quadrature_Encoder_Init(&dc_motor[1].encode, motor[1], &htim4, 100, 1);
	Quadrature_Encoder_Init(&dc_motor[2].encode, motor[2], &htim2, 100, 1);
	Quadrature_Encoder_Init(&dc_motor[3].encode, motor[3], &htim5, 100, -1);
	// motor witch dog init
	for (int i = 0; i < 4; i++)
		SEML_WitchDog_Register(&dc_motor[i], 1500, rx_timeout_Callback);
	// create timer
	SEML_Timer_Create(&timer_led_task, Timer_Single, 1000, Display_id_Callback, NULL);
	Event_Task_Handle_t motor_task_handle, can_send_task;
	Event_OS_Task_Register(&motor_task_handle, 10, EOS_High_Priority, NULL, Motor_Task, NULL);
	Event_OS_Task_Register(&can_send_task, 10, EOS_High_Priority, NULL, Can_Send_Task, NULL);
	/* USER CODE END 2 */

	/* Infinite loop */
	/* USER CODE BEGIN WHILE */
	// Event OS Run
	Event_OS_Run();
	while (1)
	{
		/* USER CODE END WHILE */

		/* USER CODE BEGIN 3 */
	}
	/* USER CODE END 3 */
}

/**
 * @brief System Clock Configuration
 * @retval None
 */
void SystemClock_Config(void)
{
	RCC_OscInitTypeDef RCC_OscInitStruct = {0};
	RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

	/** Initializes the RCC Oscillators according to the specified parameters
	 * in the RCC_OscInitTypeDef structure.
	 */
	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
	RCC_OscInitStruct.HSIState = RCC_HSI_ON;
	RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
	RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
	{
		Error_Handler();
	}

	/** Initializes the CPU, AHB and APB buses clocks
	 */
	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

	if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
	{
		Error_Handler();
	}
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
 * @brief  This function is executed in case of error occurrence.
 * @retval None
 */
void Error_Handler(void)
{
	/* USER CODE BEGIN Error_Handler_Debug */
	/* User can add his own implementation to report the HAL error return state */
	__disable_irq();
	while (1)
	{
	}
	/* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
 * @brief  Reports the name of the source file and the source line number
 *         where the assert_param error has occurred.
 * @param  file: pointer to the source file name
 * @param  line: assert_param error line source number
 * @retval None
 */
void assert_failed(uint8_t *file, uint32_t line)
{
	/* USER CODE BEGIN 6 */
	/* User can add his own implementation to report the file name and line number,
		 ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
	/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
